#include "bdd.h"
#include "qdd.h"
#include "assert.h"

// Probabalistic rotations and combinations are slightly
// different than phase based operations.

// The point of maxsq is to squeeze the most resolution as
// possible out of the NBits that we have to work with.
//
// Given a desired rotation ratio (s/p), it constructs
// sin (ss), cos(sc), and denominator(sp) values such that
//
// ss^2 + sc^2 ~= sp^2
//
// and all are representable in NBits.
//
void maxsq (int NBits, int s, int p, int *ss, int *sc, int *sp) {
  int n;
  double sn;
  n = NBits - (int) (log2(p)/2.0);
  sn = sqrt(exp2(n));
  *ss = (int) (sqrt(s)*sn);
  *sc = (int) (sqrt(p - s)*sn);
  *sp = (int) (sqrt(p)*sn);
}

void maxangle (int NBits, double r, int *c, int *s, int *d) {
  *d = (int) (sqrt(exp2(NBits)));
  *c = (int) (sqrt(exp2(NBits)) * cos(r));
  *s = (int) (sqrt(exp2(NBits)) * sin(r));
}

// Multiply a bitvector by a constant

bdd to_minterm(int index_bit_0, int max_index_bits, int int_index) {
  
  // Converts an integer into a BDD of equivalent value.
  
  bdd bdd_index = bddtrue;
  
  int i;
  int j=index_bit_0;
  for (i=0;i<max_index_bits;i++,j++) {
    if (int_index & 1) {
      bdd_index &= bdd_ithvar(j) ;
    }
    else {
      bdd_index &= bdd_nithvar(j) ;
    }
    int_index = int_index >> 1;
  }
  
  return(bdd_index);
  
}

bdd intImage(int pbits, int start, int size, int value) {
  bdd res = bdd_false();
  int i;
  int j = start;
  for (i=0;i<size;i++,j++) {
    if (value & 1 == 1) {
      res |= to_minterm(0,pbits,j);
    }
    value >>= 1;
  }
  return(res);
}

int ImageValue (int pbits, int start, int size, bdd Image) {
  int res = 0;
  int i;
  bdd x,v;
  int j = start+size-1;
  for (i=0;i<size;i++,j--) {
    x = to_minterm(0,pbits,j);
    v = bdd_restrict(Image,x);
    res <<= 1;
    if (v != bdd_false()) {
      res |= 1;
    }
  }
  return(res);
}

bdd ShiftR(int pbits, int start, int size, bdd a) {
  bdd res = bdd_false();
  bdd x,y,v;
  int i,j;
  for (i=0,j=start;i<size;i++,j++) {
    x = to_minterm(0,pbits,j+1);
    y = to_minterm(0,pbits,j);
    v = bdd_restrict(a,x);
    res = bdd_ite(y,v,res);
  }
  return res;
}

bdd Extract(int pbits, int start, int size, bdd a) {
  bdd res = bdd_false();
  bdd x,y,v;
  int i,j;
  for (i=0,j=start;i<size;i++,j++) {
    x = to_minterm(0,pbits,j);
    y = to_minterm(0,pbits,i);
    v = bdd_restrict(a,x);
    res = bdd_ite(y,v,res);
  }
  return res;
}

bdd ShiftL(int pbits, int start, int size, bdd a) {
  bdd res = bdd_false();
  bdd x,y,v;
  int i,j;
  for (i=0,j=start;i<size;i++,j++) {
    x = to_minterm(0,pbits,j);
    y = to_minterm(0,pbits,j+1);
    v = bdd_restrict(a,x);
    res = bdd_ite(y,v,res);
  }
  return res;
}

// I think you need to re-architect this.

bdd AddInto(int pbits, int start, int size, bdd a, bdd b) {
  bdd ci = bdd_false();
  bdd co = bdd_false();
  bdd r  = b;
  bdd v,va,vb;
  bdd x;
  int i;
  int j = start;
  for (i=0;i<size;i++,j++) {
    x  = to_minterm(0,pbits, j);
    va = bdd_restrict(a,x);
    vb = bdd_restrict(b,x);
    co = ci & va | ci & vb | va & vb;
    v  = va ^ vb ^ ci;
    r  = bdd_ite(x,v,r);
    ci = co;
  }
  x = to_minterm(0,pbits, j);
  r = bdd_ite(x,co,r);
  return(r);
}

bdd Mul(int pbits, int NBits, bdd M, bdd N) {
  bdd res = bdd_false();
  bdd x,v,s;
  int i;
  for (i=0;i<NBits;i++) {
    x = to_minterm(0,pbits,i);
    v = bdd_restrict(N,x);
    s = v & M;
    res = AddInto(pbits,i,NBits,s,res);
    M = ShiftL(pbits,i,NBits,M);
  }
  return res;
}

void zDump (int pbits, int SignBit, int SBits, bdd State, double Denominator) {

  bdd x,Phase,Sign,SBdd;
  int StateSize = 1 << SBits;
  int V;
  int StateIndex;
  SBdd = to_minterm(0,pbits,SignBit);
  for (StateIndex=0;StateIndex<StateSize;StateIndex++) {
    x = to_minterm(pbits,SBits,StateIndex);
    Phase = bdd_restrict(State,x);
    V = ImageValue(pbits,0,SignBit,Phase);
    Sign = bdd_restrict(Phase,SBdd);
    V *= ((Sign == bdd_false()) ? 1 : -1);
    cout << "| " << StateIndex << " > " << V * Denominator << "\n";
  }

}

void pDump (int pbits, int NBits, int SBits, bdd State, double Denominator) {

  bdd x,Phase;
  int StateSize = 1 << SBits;
  int V;
  int StateIndex;
  for (StateIndex=0;StateIndex<StateSize;StateIndex++) {
    x = to_minterm(pbits,SBits,StateIndex);
    Phase = bdd_restrict(State,x);
    V = ImageValue(pbits,0,NBits,Phase);
    cout << "| " << StateIndex << " > " << V * Denominator << "\n";
  }

}

bdd Add(int pbits, int NBits, bdd a, bdd b) {
  bdd ci = bdd_false();
  bdd co = bdd_false();
  bdd r  = bdd_false();
  bdd v,va,vb;
  bdd x;
  int i;
  for (i=0;i<NBits;i++) {
    x  = to_minterm(0,pbits, i);
    va = bdd_restrict(a,x);
    vb = bdd_restrict(b,x);
    co = ci & va | ci & vb | va & vb;
    v  = va ^ vb ^ ci;
    r  = bdd_ite(x,v,r);
    ci = co;
  }
  return(r);
}


bdd SignCheck (int pbits, int SignBit, bdd P) {
  bdd v,x,s;
  v = bdd_false();
  int i;
  for (i=0;i<SignBit;i++) {
    x  = to_minterm(0,pbits,i);
    v  |= bdd_restrict(P,x);    
  }
  x = to_minterm(0,pbits,SignBit);
  s = bdd_restrict(P,x);
  return(bdd_ite(x,v & s,P));
}

bdd SM_2C (int pbits, int SignBit, bdd P) {
  bdd ci = bdd_true();
  bdd co,x,v;
  bdd SBdd, PNeg;
  int i;
  bdd res = P;

  SBdd = to_minterm(0,pbits,SignBit);
  PNeg = bdd_restrict(P,SBdd);
  for (i=0;i<SignBit;i++) {
    x  = to_minterm(0,pbits,i);
    v  = bdd_not(bdd_restrict(res,x));
    co = v & ci;
    v  = v ^ ci;
    res  = bdd_ite(x & PNeg,v,res);
    ci = co;
  }
  return(res);
}

bdd Slice(int pbits, int start, int size, bdd Value) {
  bdd x,y,v;
  int i;
  bdd res = bdd_false();
  int j = start;
  for (i=0;i<size;i++,j++) {
    x = to_minterm(0,pbits,i);
    y = to_minterm(0,pbits,j);
    v = bdd_restrict(Value,y);
    res = bdd_ite(x,v,res);
  }
  return res;
}

// Very clever log addition property (from bdd_satcountln) .. allows
// you to avoid numeric overflow.
//
// log1p(x) = ln(1+x)
// ln(2^x + 2^y) 
//  = ln(2^x * (1+2^y/2^x))
//  = ln(2^x) + ln(1+2^(y-x))

double lnAdd (double s1, double s2) {
  double size;
  double ln2 = log1p(1.0);
  
  if (s1 < 0.0)
    size = s2;
  else if (s2 < 0.0)
    size = s1;
  else if (s1 < s2)
    size = s2 + log1p(pow(2.0,s1-s2)) / ln2;
  else
    size = s1 + log1p(pow(2.0,s2-s1)) / ln2;
  
  return size;
}

double lnCount (int pbits, int NBits, bdd S) {
  
  double lnRes = -1.0;
  double lnSat = -1.0;
  int i;
  bdd x,v;
  for (i=0;i<NBits;i++) {
    x = to_minterm(0,pbits,i);
    v = bdd_restrict(S,x);
    lnSat = bdd_satcountln(v);
    lnSat += (lnSat < 0.0) ? 0.0 : i;
    lnRes = lnAdd(lnSat,lnRes);
  }
  return(lnRes);
}

double ZChance (int pbits, int NBits, int i, bdd State) {
  
  // We use this to measure the relative probability of a selected
  // state when the state is modeled using complex numbers.

  bdd M;

  bdd x = bdd_ithvar(i);

  M = bdd_restrict(State,x);
  M = Mul(pbits,NBits,M,M);
  double Tv = lnCount(pbits,2*NBits,M);

  M = bdd_restrict(State,!x);
  M = Mul(pbits,NBits,M,M);
  double Fv = lnCount(pbits,2*NBits,M);

  double Prob;
  if (Tv < 0.0) {
    Prob = 0.0;
  } else if (Fv < 0.0) {
    Prob = 1.0;
  } else {
    double Xv = lnAdd(Tv,Fv);
    Prob = pow(2.0,Tv-Xv);
  }
  return(Prob);
}


double PChance (int pbits, int NBits, bdd x, bdd State) {

  // We use this to measure the relative probability of a selected
  // state when the state is modeled as probability values (squares
  // of the complex values).

  bdd M;

  M = bdd_restrict(State,x);
  double Tv = lnCount(pbits,2*NBits,M);

  M = bdd_restrict(State,!x);
  double Fv = lnCount(pbits,2*NBits,M);

  double Prob;
  if (Tv < 0.0) {
    Prob = 0.0;
  } else if (Fv < 0.0) {
    Prob = 1.0;
  } else {
    double Xv = lnAdd(Tv,Fv);
    Prob = pow(2.0,Tv-Xv);
  }
  return(Prob);
}

bdd ZMul(int pbits, int SignBit, int NBits, int M, bdd N) {

  bdd X,Y;
  bdd P,PSign;
  bdd SBdd, NSign, MSign;
  bdd res;

  SBdd = to_minterm(0,pbits,SignBit);

  NSign = bdd_restrict(N,SBdd);

  MSign = ((M < 0) ? bdd_true() : bdd_false());

  X = N & !SBdd;
  Y = intImage(pbits,0,NBits,abs(M));

  P = Mul(pbits,NBits,X,Y);

  PSign = MSign ^ NSign;

  // We need to correct the sign

  res = bdd_ite(SBdd,PSign,P);

  return(SignCheck(pbits,SignBit,res));
  
}

bdd PMul(int pbits, int NBits, int M, bdd N) {

  bdd Y,X;
  bdd P;

  X = N;
  Y = intImage(pbits,0,NBits,abs(M));

  P = Mul(pbits,NBits,X,Y);

  return(P);
  
}



bdd ZAdd(int pbits, int SignBit, bdd M, bdd N) {

  bdd X,Y;
  bdd Sum;

  X = SM_2C(pbits,SignBit,M);
  Y = SM_2C(pbits,SignBit,N);

  Sum = Add(pbits,SignBit+1,X,Y);

  Sum = SM_2C(pbits,SignBit,Sum);

  return(Sum);

}

bdd PAdd(int pbits, int NBits, bdd M, bdd N) {

  return(Add(pbits,NBits+1,M,N));

}

bdd ZNorm (int pbits, int SignBit, int NBits, bdd State, double *Denominator) {
  

  bdd SBdd, Sign;
  bdd XBdd, MBdd;
  int i;
  // ZNorm

  SBdd = to_minterm(0,pbits,SignBit);
  Sign = bdd_restrict(State,SBdd);
  State = State & !SBdd;

  for (i=SignBit;i>=NBits;i--) {
    cout << "i? " << i << "\n";
    XBdd = to_minterm(0,pbits,i);
    MBdd = bdd_restrict(State,XBdd);
    if (MBdd != bdd_false()) break;
  }

  cout << "ZNorm: " << 1+i-NBits << "\n";

  State = Extract(pbits,1+i-NBits,NBits,State);

  State = bdd_ite(SBdd,Sign,State);

  *Denominator *= exp2(1+i-NBits);

  return(State);

}

bdd PNorm (int pbits, int NBits, bdd State, double *Denominator) {
  

  bdd XBdd, MBdd;
  int i;
  // ZNorm

  for (i=2*NBits+1;i>=NBits;i--) {
    cout << "i? " << i << "\n";
    XBdd = to_minterm(0,pbits,i);
    MBdd = bdd_restrict(State,XBdd);
    if (MBdd != bdd_false()) break;
  }

  cout << "ZNorm: " << 1+i-NBits << "\n";

  State = Extract(pbits,1+i-NBits,NBits,State);

  *Denominator *= exp2(1+i-NBits);

  return(State);

}

bdd cPJoin (int pbits, int NBits, bdd c, bdd x, bdd State, double *Denominator) {

  int C;
  bdd v0,v10,v11;
  bdd Res = bdd_false();

  // R * | 1  0  0  0 | |00>
  //     | 0  1  0  0 | |01>
  //     | 0  0  1  1 | |10>
  //     | 0  0  0  0 | |11>

  v0 = bdd_restrict(State,!c);
  
  v10 = PAdd(pbits,NBits,bdd_restrict(State,c&!x),bdd_restrict(State,c&x));
  
  Res = !c&v0 | c&!x&v10 | c&x&bdd_false();
  
  Res = PNorm(pbits,NBits,Res,Denominator);

  return(Res);

}

bdd cPMix (int pbits, int NBits, bdd c, bdd x, bdd State, double *Denominator) {

  int C;
  bdd v0,v10,v11;
  bdd Res;

  // R * | 2  0  0  0 | |00>
  //     | 0  2  0  0 | |01>
  //     | 0  0  1::1 | |10>
  //     | 0  0  1::1 | |11>

  *Denominator /= 2;

  if (bdd_restrict(State,c& x) == bdd_false()) {
    
    v0  = PMul(pbits,NBits, 2,bdd_restrict(State,!c));
    
    v10 = bdd_restrict(State,c&!x);
    
    v11 = bdd_restrict(State,c&!x);
    
    Res = !c&v0 | c&!x&v10 | c&x&v11;
    
    Res = PNorm(pbits,NBits,Res,Denominator);
    
    return(Res);

  } else if (bdd_restrict(State,c&!x) == bdd_false()) {
    
    v0  = PMul(pbits,NBits, 2,bdd_restrict(State,!c));
    
    v10 = bdd_restrict(State,c& x);
    
    v11 = bdd_restrict(State,c& x);
    
    Res = !c&v0 | c&!x&v10 | c&x&v11;
    
    Res = PNorm(pbits,NBits,Res,Denominator);
    
    return(Res);

  } else {
    assert(0 == 1);
  }

}

bdd cPPhase (int pbits, int NBits, int S, int P, bdd c, bdd x, bdd State, double *Denominator) {

  int C;
  bdd v0,v10,v11;
  bdd Res = bdd_false();

  // R * | P  0  0  0 | |00>
  //     | 0  P  0  0 | |01>
  //     | 0  0  C::S | |10>
  //     | 0  0  S::C | |11>

  *Denominator /= P;

  if (bdd_restrict(State,c& x) == bdd_false()) {
    
    C = P - S;
    
    v0  = PMul(pbits,NBits, P,bdd_restrict(State,!c));
    
    v10 = PMul(pbits,NBits, C,bdd_restrict(State,c&!x));
    
    v11 = PMul(pbits,NBits, S,bdd_restrict(State,c&!x));
    
    Res = !c&v0 | c&!x&v10 | c&x&v11;
    
    Res = PNorm(pbits,NBits,Res,Denominator);
    
  } else if (bdd_restrict(State,c&!x) == bdd_false()) {

    C = P - S;
    
    v0  = PMul(pbits,NBits, P,bdd_restrict(State,!c));
    
    v10 = PMul(pbits,NBits, S,bdd_restrict(State,c& x));
    
    v11 = PMul(pbits,NBits, C,bdd_restrict(State,c& x));
    
    Res = !c&v0 | c&!x&v10 | c&x&v11;
    
    Res = PNorm(pbits,NBits,Res,Denominator);
    
  } else {
    assert(1 == 0);
  }

  return(Res);

}

bdd cZPhase (int pbits, int SignBit, int NBits, double r, bdd c, bdd x, bdd State, double *Denominator) {

  // R * | D  0  0  0 | |00>
  //     | 0  D  0  0 | |01>
  //     | 0  0  C -S | |10>
  //     | 0  0  S  C | |11>

  // S/D = sin(phi);
  // C/D = cos(phi);

  bdd v0, v10, v11;
  bdd Res;
  bdd Sign;

  int C,S,D;

  maxangle(NBits,r,&C,&S,&D);

  cout << "S = " << S << "\n";
  cout << "C = " << C << "\n";
  cout << "D = " << D << "\n";

  *Denominator /= D;

  v0  = ZMul(pbits,SignBit,NBits,D,bdd_restrict(State,!c));

  v10 = ZAdd(pbits,SignBit,
	     ZMul(pbits,SignBit,NBits, C,bdd_restrict(State,c&!x)),
	     ZMul(pbits,SignBit,NBits,-S,bdd_restrict(State,c& x)));

  v11 = ZAdd(pbits,SignBit,
	     ZMul(pbits,SignBit,NBits, S,bdd_restrict(State,c&!x)),
	     ZMul(pbits,SignBit,NBits, C,bdd_restrict(State,c& x)));

  Res = !c&v0 | c&!x&v10 | c&x&v11;

  Res = ZNorm(pbits,SignBit,NBits,Res,Denominator);

  return(Res);

}

bdd Load (int pbits, int SignBit, int NBits, int SBits, int StateIndex, int V, bdd State) {
  bdd x,SBdd,VSign,VBdd;

  x = to_minterm(pbits,SBits,StateIndex);
  SBdd = to_minterm(0,pbits,SignBit);
  VSign = ((V < 0) ? SBdd : bdd_false());
  VBdd = intImage(pbits,0,NBits,abs(V));
  return(bdd_ite(x,VSign | VBdd,State));
}

bdd cMix3_4 (int pbits, int NBits, bdd c, bdd x, bdd y, bdd State, double *Denominator) {
  *Denominator /= 3;
  assert(bdd_restrict(State,c &  x &  y) == bdd_false());
  assert(bdd_restrict(State,c & !x &  y) == bdd_false());
  assert(bdd_restrict(State,c &  x & !y) == bdd_false());
  bdd v0  = PMul(pbits,NBits,3,bdd_restrict(State,!c));
  bdd v10 = bdd_restrict(State,!x & !y);
  State = !c&v0 | c&bdd_ite(x&y,State,v10);
  State = PNorm(pbits,NBits,State,Denominator);
  return(State);
}

int main () {

  bdd_init(2000,1000);
  bdd_setvarnum(9);

  int pbits = 5;
  int SignBit = 31;
  int NBits = 12;
  int SBits = 4;
  double r;
  double D = 1.0;
  
  bdd c,x;

  bdd State = bdd_false();

  D /= exp2(NBits-1);

  State = Load(pbits,SignBit,NBits,SBits,3,-(int)exp2(NBits-1),State);
  State = Load(pbits,SignBit,NBits,SBits,8, (int)exp2(NBits-1),State);

  ZNorm(pbits,SignBit,NBits,State,&D);

  c = bdd_nithvar(pbits+1);
  x = bdd_ithvar(pbits);

  r = 2.0*M_PI*(30.0/360.0);

  zDump(pbits,SignBit,SBits,State,D);


  // I think for two phase systems (like the Grover algorithm) a dual
  // denominator would be helpful.  There would be a greater
  // denominator and a lesser denominator.  The lesser denominator
  // would track values that are approaching zero while the greater
  // denominator would track values approaching one.  When the two
  // are close enough, they may bleed values between them.  

  // Perhaps the user could adjust the lesser denominator manually (?)

  int i;
  for (i=0;i<12;i++) {
    
    cout << "D = " << D << "\n";

    State = cZPhase(pbits,SignBit,NBits,r,c,x,State,&D);
    
    zDump(pbits,SignBit,SBits,State,D);
    
  }

  // Probabalistic Operation

  bdd y = bdd_ithvar(pbits+1);
  bdd z = bdd_ithvar(pbits+2);

  cout << "\n";
  cout << "Probabalistic Operation:\n";
  cout << "\n";

  State = bdd_false();

  D = 1.0/exp2(NBits-1);
  
  State = Load(pbits,SignBit,NBits,SBits,0,(int)exp2(NBits-1),State);
  
  pDump(pbits,NBits,SBits,State,D);

  cout << "\n";

  State = PNorm(pbits,NBits,State,&D);

  pDump(pbits,NBits,SBits,State,D);

  cout << "\n";

  cout << "x : " << PChance(pbits,NBits,x,State) << "\n";

  cout << "y : " << PChance(pbits,NBits,y,State) << "\n";

  State = cPMix(pbits,NBits,bdd_true(),x,State,&D);

  pDump(pbits,NBits,SBits,State,D);

  cout << "\n";

  cout << "x : " << PChance(pbits,NBits,x,State) << "\n";

  cout << "y : " << PChance(pbits,NBits,y,State) << "\n";

  State = cPMix(pbits,NBits,bdd_true(),y,State,&D);

  pDump(pbits,NBits,SBits,State,D);

  cout << "\n";

  cout << "x : " << PChance(pbits,NBits,x,State) << "\n";

  cout << "y : " << PChance(pbits,NBits,y,State) << "\n";

  State = cPJoin(pbits,NBits,bdd_true(),x,State,&D);

  pDump(pbits,NBits,SBits,State,D);

  cout << "\n";

  cout << "x : " << PChance(pbits,NBits,x,State) << "\n";

  cout << "y : " << PChance(pbits,NBits,y,State) << "\n";

  State = cPJoin(pbits,NBits,bdd_true(),y,State,&D);

  pDump(pbits,NBits,SBits,State,D);

  cout << "\n";

  cout << "x : " << PChance(pbits,NBits,x,State) << "\n";

  cout << "y : " << PChance(pbits,NBits,y,State) << "\n";

  State = cPPhase(pbits,NBits,3,5,bdd_true(),x,State,&D);

  pDump(pbits,NBits,SBits,State,D);

  cout << "\n";

  cout << "x : " << PChance(pbits,NBits,x,State) << "\n";

  cout << "y : " << PChance(pbits,NBits,y,State) << "\n";

  State = cPPhase(pbits,NBits,7,13,bdd_true(),y,State,&D);

  pDump(pbits,NBits,SBits,State,D);

  cout << "\n";

  cout << "x : " << PChance(pbits,NBits,x,State) << "\n";

  cout << "y : " << PChance(pbits,NBits,y,State) << "\n";

  State = cPJoin(pbits,NBits,bdd_true(),x,State,&D);
  State = cPJoin(pbits,NBits,bdd_true(),y,State,&D);

  pDump(pbits,NBits,SBits,State,D);

  cout << "\n";

  cout << "x : " << PChance(pbits,NBits,x,State) << "\n";

  cout << "y : " << PChance(pbits,NBits,y,State) << "\n";

  State = cPMix(pbits,NBits,bdd_true(),x,State,&D);

  State = cMix3_4(pbits,NBits,!x,y,z,State,&D);

  pDump(pbits,NBits,SBits,State,D);

  cout << "\n";

  cout << "x : " << PChance(pbits,NBits,x,State) << "\n";

  cout << "y : " << PChance(pbits,NBits,y,State) << "\n";

  exit(0);

}


